﻿using UnityEngine;
using System.Collections.Generic;
namespace DynamicRagdoll {

    /*
        Module than enables going ragdoll on collision checks
        
        1. checks for incoming rigidbody collisions, our when we run into something,
            with enough force, if they're above the threshold, we go ragdoll

        2. when ragdolling, we check for collisions on the bones to add bone decay
            (through the attached RagdollController) based on teh magnitude of the collisions    
    */

    [System.Serializable] public class CollisionRagdoller 
    {
        public bool enabled = true;
        
        RagdollController ragdollController;

        //contacts generated by ragdoll collision enter
        ContactPoint[] contacts = new ContactPoint[5];
        int contactCount;

        // planar velocity (so we dont ragdoll on steps)
        Vector3 controllerVelocity {
            get {
                Vector3 velocity = getVelocityCallback();
                velocity.y = 0;
                return velocity;
            }
        }
        System.Func<Vector3> getVelocityCallback;

        public void SetGetVelocityCallback (System.Func<Vector3> getVelocityCallback) {
            this.getVelocityCallback = getVelocityCallback;
        }

        public void InitializeRagdollOnCollisions (RagdollController ragdollController) {
            this.ragdollController = ragdollController;
            //subscribe to receive a callback on ragdoll bone collision
			ragdollController.ragdoll.onCollisionEnter += OnRagdollCollisionEnter;		
        }

        bool CollisionIsAboveStepOffset (Collision collision, float stepOffset, float buffer) {
            // if it's below our step offset (plus a buffer)
            // ignore it, we can just step on top of it

            float offsetThreshold = stepOffset + buffer;
            contactCount = collision.GetContacts(contacts);

            for (int i = 0; i < contactCount; i++) {

                float yOffset = contacts[i].point.y - ragdollController.transform.position.y;
                if (yOffset > offsetThreshold)
                    return true;
            }
            return false;
        }

        bool CollisionIsAboveCrushOffset (Collision collision, float charHeight) {
            for (int i = 0; i < contactCount; i++) {
                float yOffset = contacts[i].point.y - ragdollController.transform.position.y;

                if (charHeight - yOffset < ragdollController.profile.crushMassTopOffset) {
                    return true;
                }
            }
            return false;
        }
        
        bool CollisionHasCrushMass (Collision collision) {
            return collision.collider.attachedRigidbody != null && collision.collider.attachedRigidbody.mass >= ragdollController.profile.crushMass;
        }
        
        /*
			callback called when ragdoll bone gets a collision
            then apply bone decay to those bones
		*/    
		void OnRagdollCollisionEnter(RagdollBone bone, Collision collision)
		{
            if (!enabled) {
                return;
            }

            //maybe add warp to master state for ragdoll check
            
            bool checkForRagdoll = ragdollController.state == RagdollControllerState.Animated || ragdollController.state == RagdollControllerState.BlendToAnimated;
            
            bool isFalling = ragdollController.state == RagdollControllerState.Falling;

			if (!isFalling && !checkForRagdoll)
				return;

            //check for and ignore self ragdoll collsion (only happens when falling)
            if (ragdollController.ragdoll.ColliderIsPartOfRagdoll(collision.collider))
                return;
            
            if (checkForRagdoll) {

                //if we're getting up, knock us out regardless of where the collision takes place
                if (!ragdollController.isGettingUp) {

                    // if it's below our step offset (plus a buffer)
                    // ignore it... we can just step on top of it
                    if (!CollisionIsAboveStepOffset(collision, ragdollController.charStepOffset, .1f))
                        return;
                }
            }
            

            bool isCrushed = CollisionHasCrushMass(collision) && CollisionIsAboveCrushOffset(collision, ragdollController.charHeight);
			float collisionMagnitude2 = collision.relativeVelocity.sqrMagnitude;
            
            if (checkForRagdoll) {

                // string message = "crush";
                // check if we're being crushed
                bool goRagdoll = isCrushed;

                // else check if the collision is above our incoming threhsold, (something being thrown at us)
                if (!goRagdoll) {
                    // message = "incoming";
                    goRagdoll = collisionMagnitude2 >= ragdollController.profile.incomingMagnitudeThreshold * ragdollController.profile.incomingMagnitudeThreshold;
                }
                
                // else check if we're travelling fast enough to go ragdoll (we ran into a wall or something...)
                if (!goRagdoll){
                    // message = "outgoing";
                    collisionMagnitude2 = controllerVelocity.sqrMagnitude;
                    goRagdoll = collisionMagnitude2 >= ragdollController.profile.outgoingMagnitudeThreshold * ragdollController.profile.outgoingMagnitudeThreshold;
                }
    
                if (!goRagdoll)
                    return;

                // Debug.Log( message + "/" + bone.name + " went ragdoll cuae of " + collision.collider.name + "/" + Mathf.Sqrt(collisionMagnitude2));
                ragdollController.GoRagdoll("from collision");
            }

            HandleBoneDecayOnCollision(collisionMagnitude2, bone, isCrushed, collision);
		}

        void HandleBoneDecayOnCollision (float collisionMagnitude2, RagdollBone bone, bool isCrushed, Collision collision) {

            if (isCrushed) {
                //Debug.LogWarning(bone + " / " + collision.transform.name + " CrUSHED");
                ragdollController.AddBoneDecay(bone.bone, 1, ragdollController.profile.neighborDecayMultiplier);
            }
            
            // if the magnitude is above the minimum threshold for adding decay
            else if (collisionMagnitude2 >= ragdollController.profile.decayMagnitudeRange.x * ragdollController.profile.decayMagnitudeRange.x) {
                float magnitude = Mathf.Sqrt(collisionMagnitude2);

                //linearly interpolate decay between 0 and 1 base on collision magnitude
                
                float linearDecay = (magnitude - ragdollController.profile.decayMagnitudeRange.x) / (ragdollController.profile.decayMagnitudeRange.y -  ragdollController.profile.decayMagnitudeRange.x);
                
                //Debug.Log(bone + " / " + collision.transform.name + " mag: " + magnitude + " decay " + linearDecay);
                ragdollController.AddBoneDecay(bone.bone, linearDecay, ragdollController.profile.neighborDecayMultiplier);
            }
        }
    }
}